// File: src/imu_publisher.cpp
#include "slam_robot/imu_publisher.hpp"

// 使用 C++ 标准时钟字面量
using namespace std::chrono_literals;

ImuPublisher::ImuPublisher(rclcpp::Node* parent_node, std::shared_ptr<SerialBridge> serial)
    : parent_node_(parent_node), serial_(serial) {  
    // 使用父节点的create_publisher和create_wall_timer
    publisher_ = parent_node_->create_publisher<sensor_msgs::msg::Imu>("imu", 10);
    timer_ = parent_node_->create_wall_timer(
        std::chrono::milliseconds(10),  // 注意使用std::chrono
        std::bind(&ImuPublisher::publishImu, this));
}

void ImuPublisher::publishImu() {
    auto data = serial_->getImuData();
    auto msg = sensor_msgs::msg::Imu();

    msg.header.stamp = parent_node_->now();
    msg.header.frame_id = "imu_link";

    msg.linear_acceleration.x = data.accel_x;  // 正确访问字段
    msg.linear_acceleration.y = data.accel_y;
    msg.linear_acceleration.z = data.accel_z;

    msg.angular_velocity.x = data.gyro_x;
    msg.angular_velocity.y = data.gyro_y;
    msg.angular_velocity.z = data.gyro_z;

    publisher_->publish(msg);
}